#!/usr/bin/env roseus

(setq ros::*compile-message* t) ;; switch for compile
(ros::load-ros-manifest "jskpointcloud")
(load "package://jskpointcloud/euslisp/jskpointcloud.l")
(ros::load-ros-manifest "human_tracking")

(defvar *r-max* 255)
(defvar *r-min* 50)
(defvar *g-max* 30)
(defvar *g-min* 0)
(defvar *b-max* 30)
(defvar *b-min* 0)
(defvar *num-thr* 1000)

(defun pcloud-cb (msg)
  ;;(pprint 'capture)
  (let ((3dp
         (make-3dpointcloud-from-msg2 msg :remove-nan t))
        (cnt (* (expt (sqrt (* 2 pi)) 3) 40.0 0.16 0.25))
        (3dp-lst (list)))
    (dolist (l *indices*)
      (let ((p (send 3dp :filter-with-indices (coerce l cons) :create t)))
        (when t;; (is-person-with-color-object p)
          (send p :nfilter :key
                #'(lambda (x)
                    (not (and (< 0 (elt x 0)) (< (elt x 0) 0)))))
          (push p 3dp-lst))))
    (when 3dp-lst
      (let* ((cds (send *tf-listener* :lookup-transform
                        *base-frame*
                        (send msg :header :frame_id)
                        (ros::time 0)))
             (pub-3dp (instance 3dpointcloud :init :points
                                (delete nil
					(mapcar #'(lambda (x)
						    (let* ((cen (send x :centroid))
							   (xx (elt cen 0)))
						      (if (and (< xx 0) (< 0 xx))
							  nil
							(send cds :transform-vector cen))))
						3dp-lst))))
	     (pub-pc (make-msg-from-3dpointcloud pub-3dp :frame *base-frame*)))
        ;;(print (list cds (send pub-3dp :points)))
        (if (send pub-3dp :points) (ros::publish *pub-people-topic* pub-pc))))))

(defun is-person-with-color-object (human-3dp)
  (let ((color-obj-3dp
         (send human-3dp :nfilter :ckey
               #'(lambda (x)
                   (let ((col (scale 255 x)))
                     (and (< *r-min* (elt col 0) *r-max*)
                          (< *g-min* (elt col 1) *g-max*)
                          (< *b-min* (elt col 2) *b-max*))))
               :create t)))
    (< *num-thr* (print (send color-obj-3dp :size)))))

(defun index-cb (msg)
  (let ((hdr (send msg :header))
        (lst (send msg :array)))
    (pprint (list 'index (length lst)))
    (setq *indices* nil)
    (dolist (l lst)
      (push (send l :indices) *indices*))))

(ros::roseus "body_subscriber")
(ros::subscribe "/camera/depth/points" sensor_msgs::PointCloud2 #'pcloud-cb)
(ros::subscribe "/openni_tracker/indices" human_tracking::IndicesArray #'index-cb)
(setq *sensor-frame* "/camera_rgb_optical_frame")
(setq *pub-people-topic* "people_with_socks")
(setq *tf-listener* (instance ros::transform-listener :init))
(setq *base-frame* "/base_footprint")
(ros::advertise *pub-people-topic* sensor_msgs::PointCloud 1)

(setq *indices* nil)
(setq *cds* (make-coords))

(defun ros-spin ()
  (do-until-key (ros::spin-once)))

(ros-spin)